%> @file cc_mpc_coldstart_matrixevaluation_sparse.m
%> @brief The function evaluates matrices like Hessian and F-matrix for the MPC controller.
%> 
%> @author Mikhail Konnik
%> @date   12 January 2012
%>
%> @section coldstartmatrixevaluation Cold Start of the MPC controller - Hessian and F-matrix
%> Using a standard state space RHC formulation, the cost function \f$V_{N_p,N_c}\f$ for a state prediction horizon \f$N_p\f$ and control prediction horizon \f$N_c\f$ is:
%> 
%> \f$ V_{N_p,N_c} = \frac{1}{2}x^TC^T Cx + \frac{1}{2} X^T \mathcal Q X + \frac{1}{2} U^T \mathcal R U, \,\,\,\,\,\,\, X = [x_1, x_2, \dots x_{N_p}]^T, \,\,\,\,\,\,\,   U = [u_0, u_1, \dots u_{N_c}]^T \f$
%> 
%> where \f$x\f$ denotes the current state (i.e., \f$x=x_0\f$). The matrices \f$\mathcal Q$ and \f$ \mathcal R$ are defined as:
%> 
%> \f$\mathcal Q = diag\{C^T C,C^T C,\dots P\} \mbox{ and } \mathcal R = diag\{R, R, \dots R\}, \f$
%> 
%> where the matrix \f$R\f$ is the penalty for the size of the control input, and the matrix \f$P\f$ is the penalty for missing the desired goal state (if the desired goal is zero). The penalty matrix for excessive control energy is \f$R = 10^{-12}\cdot I\f$.  The horizons were set to  \f$N_p=2\f$ for the state prediction horizon and \f$N_c =1\f$ for the control prediction horizon.
%> 
%> Define matrices \f$\Gamma\f$ and \f$\Omega\f$ as:
%> 
%> \f$ \Gamma = \left[ \begin{array}{cccc} B	&	0	& \dots 	&0\\ AB	&	B	& \dots 	&0\\ \vdots	&	\vdots	& \ddots 	&\vdots\\  A^{N_p-1}B&  A^{N_p-2}B& \dots		&  A^{N_p-N_c}B\\ \end{array} \right] \f$
%> 
%> and 
%> 
%> \f$ \Omega = \left[ \begin{array}{c} A \\ A^2 \\ \vdots \\ A^{N_p}  \\ \end{array} \right]  \f$
%> 
%> The dynamics in \eqref{eq:sys} can be expressed over the prediction horizon in a vector form as:
%> 
%> \f$ X = \Gamma U + \Omega x. \f$
%> 
%> Substituting  into the cost function  we obtain:
%> 
%> \f$ V_{N_p,N_c} = \bar{V} + \frac{1}{2} U^T \mathbb H U + U^T\mathbb F x,\f$
%> 
%> where the term \f$\bar{V}\f$ is independent of $U$. The Hessian matrix \f$\mathbb{H}\f$ and the matrix \f$\mathbb{F}\f$ are defined as follows:
%> 
%> \f$ \mathbb{H} \triangleq \Gamma^T \mathcal Q \Gamma + \mathcal R, \,\,\,\,\,\,\   \mathbb{F} \triangleq   \Gamma^T \mathcal Q \Omega .\f$
%> 
%======================================================================
%> @param A_e		= states evolution matrix.
%> @param B_e		= input control matrix.
%> @param C_e			= Output control matrix
%> @param Nc			= control prediction horizon.
%> @param Np			= state prediction horizon.
%> @retval H			= Hessian matrix for the MPC
%> @retval F			= F-matrix for the MPC
% ======================================================================
function [H, F] = cc_mpc_coldstart_matrixevaluation_sparse(A_e,B_e,C_e,Nc,Np)

%%%%%%%%%% Weighting matrices for the dmpc commnad and MPC computation %%%%%%
     R = 10^(-12)*speye(size(B_e,2));  %%% penalty for the excessive control energy;

     P = 10^(-1)*speye(size(A_e,1)); %% penalty for missing the desired terminal state
%%%%%%%%%% Weighting matrices for the dmpc commnad and MPC computation %%%%%%


[OMEGA,GAMMA] =  dedonahessian(A_e,B_e,C_e,Nc,Np); %%% <----- Invoking the function to calculate GAMMA and OMEGA for the Hessian.


%%%%%%%%%% START: Filling the matrix Q_bold <----- SPARCE MODE
[row_A_e,col_A_e] = size(A_e);

CQtC = C_e'*C_e; %%% See De Dona book on Chapter 5, page 106 for the details.

Q_bold_sparse =  sparse(Np*row_A_e,Np*row_A_e);
Q_bold_sparse(1:row_A_e,1:col_A_e) = CQtC;  %% this is the first block of the Q-matrix needed for Hessian calculation.


	for ii = 2:(Np-1)
	Q_bold_sparse((row_A_e*(ii-1)+1):(row_A_e*(ii)), (col_A_e*(ii-1)+1):(col_A_e*(ii))) = CQtC;
	end %%%for ii = 1:size(Q,1)


%%% start: Last element on the diagonal must be a P matrix, which is a penalty for the missing states.
[row_Q_bold,col_Q_bold] = size(Q_bold_sparse);
Q_bold_sparse((row_Q_bold-row_A_e+1):row_Q_bold, (col_Q_bold-row_A_e+1):col_Q_bold) = P;
%%%   end: Last element on the diagonal must be a P matrix, which is a penalty for the missing states.
%%%%%%%%%% END: Filling the matrix Q_bold <----- SPARCE MODE



	%%%%%%%%%% ## START: Filling the matrix R_bold
	[row_B_e,col_B_e] = size(B_e);
	R_bold =  sparse(Nc*col_B_e,Nc*col_B_e);
	R_bold(1:col_B_e,1:col_B_e) = R;

	for ii = 2:Nc
	R_bold((col_B_e*(ii-1)+1):(col_B_e*(ii)), (col_B_e*(ii-1)+1):(col_B_e*(ii))) = R;
	end %%%for ii = 1:size(Q,1)
	%%%%%%%%%% #### END: Filling the matrix R_bold




%%%%%%%%%%%%%% #### START: Constructing Hessian matrix for the Model Predictive Control. %%%%%%%%
transposed_Gamma = GAMMA';
H = (transposed_Gamma)*Q_bold_sparse*GAMMA + R_bold;  %%% <---- HESSIAN matrix
F = (transposed_Gamma)*Q_bold_sparse*OMEGA;
%%%%%%%%%%%%%% #### START: Constructing Hessian matrix for the Model Predictive Control. %%%%%%%%



%  	figure(115), spy(OMEGA); viz_print_figure_to_eps(115,strcat('mpc_MIMO_unconstrained_49c_OMEGAmatrix'));
%  	figure(116), spy(GAMMA); viz_print_figure_to_eps(116,strcat('mpc_MIMO_unconstrained_49c_GAMMAmatrix'));


%  	figure(117), spy(Q_bold_sparse); viz_print_figure_to_eps(117,strcat('mpc_MIMO_unconstrained_49c_Q_bold_sparsematrix'));
%
%  	figure(118), spy(A_e); viz_print_figure_to_eps(118,strcat('mpc_MIMO_unconstrained_49c_A_ematrix'));




%%%%%% #### THIS IS THE BEGINNING OF FUNCTION function [OMEGA,GAMMA] =  dedonahessian(A_e,B_e,C_e,Nc,Np) %%%%%%%%
function [OMEGA,GAMMA] =  dedonahessian(A_e,B_e,C_e,Nc,Np);

%%%% START: Procedure of filling the matrix OMEGA
[row_A_e,col_A_e] = size(A_e);

OMEGA=A_e;
v = speye(row_A_e)*B_e;

for kk=2:Np

    v( (1+(kk-1)*row_A_e):(kk*row_A_e),:)= A_e^(kk-1)*B_e;
    OMEGA( (1+(kk-1)*row_A_e):(kk*row_A_e),:)= A_e^kk;

end %% for kk=2:Np
%%%% ## END: Procedure of filling the matrix OMEGA



%%%% START: Procedure of filling the matrix Phi
[row_B_e,col_B_e] = size(B_e);

GAMMA=sparse(Np*row_B_e,Nc*col_B_e); %declare the dimension of Phi
GAMMA(:,1:col_B_e)=v;        % first column of Phi

for ii=2:Nc

    rownum = row_B_e*(Np-ii+1);

    GAMMA(:,(1+col_B_e*(ii-1)):(col_B_e*ii))=[sparse(row_B_e*(ii-1), col_B_e); v(1:rownum,1:col_B_e)]; %Toeplitz matrix

end %%% for ii=2:Nc
%%%%%% #### THIS IS THE END OF FUNCTION function [OMEGA,GAMMA] =  dedonahessian(A_e,B_e,C_e,Nc,Np) %%%%%%%%%%%%%%%%%